
#include "IAlgorithm.h"
#include "IContinuousConvexCast.h"
#include "IGjkPairDetector.h"
#include "IDetectorResult.h"
#include "BGridPlane"
#include "member_BShape.h"

IContinuousConvexCast::IContinuousConvexCast(BShape* convexA, BShape* convexB)
{
	_shapeA = convexA;
	_shapeB = convexB;
}

void IContinuousConvexCast::computeClosestPoints(const BMatrix& matrixA, const BMatrix& matrixB, IDetectorResult& result)
{
	if (_shapeB)
	{
		_simplexSolver.reset();
		IGjkPairDetector gjk(_shapeA, _shapeB, &_simplexSolver);
		gjk.closestPoints(matrixA, matrixB, result);
	}
}

bool IContinuousConvexCast::calcTimeOfImpact(const BMatrix& sourceA, const BMatrix& targetA, const BMatrix& sourceB, const BMatrix& targetB, CastResult& caseResult)
{
	BVector linVelA, angVelA;
	CalculateVelocity(sourceA, targetA, linVelA, angVelA);

	BVector linVelB, angVelB;
	CalculateVelocity(sourceB, targetB, linVelB, angVelB);

	BReal boundingRadiusA = _shapeA->box().radius();
	BReal boundingRadiusB = _shapeB ? _shapeB->box().radius() : 0;

	BReal maxAngularProjectedVelocity = angVelA.length() * boundingRadiusA + angVelB.length() * boundingRadiusB;
	BVector relLinVel = linVelB - linVelA;

	BReal relLinVelocLength = relLinVel.length();
	if ((relLinVelocLength + maxAngularProjectedVelocity) == 0)
		return false;

	IDetectorResult result;
	computeClosestPoints(sourceA, sourceB, result);
	if (result.hasResult)
	{
		BReal dist = result.distance;
		BVector point = result.pointB;
		BVector normal = result.normalB;
		BReal projectedLinearVelocity = relLinVel.dot(normal);
		if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= REAL_EPSILON)
			return false;

		int numIter = 0;
		BReal fraction = 0;
		while (dist > REAL_EPSILON)
		{
			projectedLinearVelocity = relLinVel.dot(normal);
			if ((projectedLinearVelocity + maxAngularProjectedVelocity) <= REAL_EPSILON)
				return false;

			fraction += dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
			if (fraction < 0 || fraction > 1)
				return false;

			BMatrix interpMatrixA = IntegrateTransform(sourceA, linVelA, angVelA, fraction);
			BMatrix interpMatrixB = IntegrateTransform(sourceB, linVelB, angVelB, fraction);
			computeClosestPoints(interpMatrixA, interpMatrixB, result);
			if (result.hasResult)
			{
				dist = result.distance;
				point = result.pointB;
				normal = result.normalB;
			}
			else
			{
				return false;
			}
			if (numIter++ > 64)
			{
				return false;
			}
		}

		caseResult.fraction = fraction;
		caseResult.normal = normal;
		caseResult.point = point;
		return true;
	}

	return false;
}
